#include <ros/ros.h>
#include <std_msgs/Float64.h>

namespace inv_pen
{
    class VelPub
    {
    public:
        double get_rate(void);
        void loop(void);
        VelPub(std::string ns);

    private:
        double situ_acc;
        double angle_acc;
        double dt;
        double rate;
        std_msgs::Float64 situ_vel;
        std_msgs::Float64 angle_vel;
        ros::NodeHandle pnh;
        ros::Subscriber situ_acc_sub;
        ros::Subscriber angle_acc_sub;
        ros::Publisher situ_vel_pub;
        ros::Publisher angle_vel_pub;
        void SituAccCallBack(const std_msgs::Float64 &msg);
        void AngleAccCallBack(const std_msgs::Float64 &msg);
    };
}

inv_pen::VelPub::VelPub(std::string ns) : situ_acc(0.0), angle_acc(0.0), dt(1.0/30.0), rate(30)
{
    situ_vel.data = 0.0;
    angle_vel.data = 0.0;
    pnh = ros::NodeHandle(ns);
    situ_acc_sub = pnh.subscribe("/situ_acc", 1, &inv_pen::VelPub::SituAccCallBack, this);
    angle_acc_sub = pnh.subscribe("/angle_acc", 1, &inv_pen::VelPub::AngleAccCallBack, this);
    situ_vel_pub = pnh.advertise<std_msgs::Float64>("/situ_vel", 1);
    angle_vel_pub = pnh.advertise<std_msgs::Float64>("/angle_vel", 1);
}

void inv_pen::VelPub::SituAccCallBack(const std_msgs::Float64 &msg)
{
    situ_acc = msg.data;
    ROS_INFO("recieve situ_acc:%f", situ_acc);
}

void inv_pen::VelPub::AngleAccCallBack(const std_msgs::Float64 &msg)
{
    angle_acc = msg.data;
}

void inv_pen::VelPub::loop(void)
{
    ros::spinOnce();
    /*
    接收从acc-pub传来的加速度信息，进行积分
    */
    situ_vel.data += situ_acc * dt;
    angle_vel.data += angle_acc * dt;
    situ_vel_pub.publish(situ_vel);
    angle_vel_pub.publish(angle_vel);
    ROS_INFO("pub vel:%f\t%f", situ_vel.data, angle_vel.data);
}

double inv_pen::VelPub::get_rate(void)
{
    return rate;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "vel_pub_node");
    inv_pen::VelPub velpub("");
    ros::Rate rate(velpub.get_rate());
    while (ros::ok())
    {
        velpub.loop();
        rate.sleep();
    }
    ros::shutdown();
    return 0;
}